# -*- coding: utf-8 -*-
import json
import numpy as np
import os
import sys

import IPython

#import meshpy.obj_file as obj_file
import trimesh

from autolab_core import RigidTransform

GRIPPER_PARAMS_FILENAME = 'params.json'
T_MESH_GRIPPER_FILENAME = 'T_mesh_gripper.tf' 
T_GRASP_GRIPPER_FILENAME = 'T_grasp_gripper.tf' 


class RobotGripper(object):
    """ Robot gripper wrapper for collision checking and encapsulation of grasp parameters (e.g. width, finger radius, etc)
    Note: The gripper frame should be the frame used to command the physical robot
    
    Attributes
    ----------
    name : :obj:`str`
        name of gripper
    mesh : :obj:`Mesh3D`
        3D triangular mesh specifying the geometry of the gripper
    params : :obj:`dict`
        set of parameters for the gripper, at minimum (finger_radius and grasp_width)
    T_mesh_gripper : :obj:`RigidTransform`
        transform from mesh frame to gripper frame (for rendering)
    T_grasp_gripper : :obj:`RigidTransform`
        transform from gripper frame to the grasp canonical frame (y-axis = grasp axis, x-axis = palm axis)
    """

    def __init__(self, name, gripper_mesh, finger_mesh, base_mesh, gripper_mesh_filename, params):
        self.name = name
        self.mesh = gripper_mesh
        self.finger_mesh = finger_mesh
        self.base_mesh = base_mesh
        self.mesh_filename = gripper_mesh_filename
        # self.T_mesh_gripper = T_mesh_gripper
        # self.T_grasp_gripper = T_grasp_gripper
        for key, value in params.items():
            setattr(self, key, value)

    @staticmethod
    def load(yaml_config):
        """ Load the gripper specified by gripper_name.

        Parameters
        ----------
        gripper_name : :obj:`str`
            name of the gripper to load
        gripper_dir : :obj:`str`
            directory where the gripper files are stored

        Returns
        -------
        :obj:`RobotGripper`
            loaded gripper objects
        """
        gripper_name = yaml_config['gripper']['name']
        gripper_dir = yaml_config['gripper']['dir']
        GRIPPER_MESH_FILENAME = yaml_config['gripper']['gripper_mesh_filename']
        FINGER_MESH_FILENAME = yaml_config['gripper']['finger_mesh_filename']
        BASE_MESH_FILENAME = yaml_config['gripper']['base_mesh_filename']
        gripper_mesh_filename = os.path.join(gripper_dir, gripper_name, GRIPPER_MESH_FILENAME)
        finger_filepath = os.path.join(gripper_dir, gripper_name, FINGER_MESH_FILENAME)
        base_filepath = os.path.join(gripper_dir, gripper_name, BASE_MESH_FILENAME)
        coarse_finger_mesh = trimesh.load_mesh(finger_filepath)
        coarse_base_mesh = trimesh.load_mesh(base_filepath)

        gripper_mesh = trimesh.load_mesh(gripper_mesh_filename)
        f = open(os.path.join(os.path.join(gripper_dir, gripper_name, GRIPPER_PARAMS_FILENAME)), 'r')
        params = json.load(f)

        # T_mesh_gripper = RigidTransform.load(os.path.join(gripper_dir, gripper_name, T_MESH_GRIPPER_FILENAME))
        # T_grasp_gripper = RigidTransform.load(os.path.join(gripper_dir, gripper_name, T_GRASP_GRIPPER_FILENAME))
        # return RobotGripper(gripper_name, mesh, mesh_filename, params, T_mesh_gripper, T_grasp_gripper)
        return RobotGripper(gripper_name, gripper_mesh, coarse_finger_mesh, coarse_base_mesh, gripper_mesh_filename, params)
